import java.util.Arrays;


public class Robot {
	
	private Wheel wheelA;
	private Wheel wheelB; 
	
	private double distanceBetweenWheels = 10;
	private double initial_x = 0;
	private double initial_y = 0;
	private double initial_angle = 0; //rad
	
	private double x_val = 0;
	private double y_val = 0;
	
	public Robot() {
		wheelA = new Wheel(5);
		wheelB = new Wheel(5);
		
		setX(0);
		setY(0);
		
		go();
	}
	
	public double getX() {
		return x_val;
	}
	
	public void setX(double x) {
		x_val = x;
	}
	
	public double getY() {
		return y_val;
	}
	
	public void setY(double y) {
		y_val = y;
	}

	public void go() {

		for (double time = 0; time < 10; time += 0.5) {
			/*
			 * SIMPLIFICATION:
			 * use this to calculate movement after a unit of time
			 */
			System.out.println(Arrays.toString(calcLoc(1)));
		}
	}
	
	/*public double[] calcLocOLD(double time) {
		
		double speedA = wheelA.getSpeed();
		double speedB = wheelB.getSpeed();
		
		if (speedA == speedB) {
			wheelB.setSpeed(speedB + 0.0000001);
			// this tries to avoid division by 0
			// but its statistically likely to fail
			// when questioned, say its a feature
		}
		
		double fraction = 
				(distanceBetweenWheels * (speedA + speedB)) 
				/ (2 * (speedA - speedB));
		double trig_x = 
				Math.sin((speedA - speedB) * time
						/ distanceBetweenWheels + initial_angle) - Math.sin(initial_angle);
		double trig_y = 
				Math.cos((speedA - speedB) * time
						/ distanceBetweenWheels + initial_angle) - Math.cos(initial_angle);
		
		double x = initial_x + fraction * trig_x;
		double y = initial_y + fraction * trig_y;
		
		double arr[] = {x,y};
		return arr;
	}*/
	
public double[] calcLoc(double time) {
	
	/*
	 * SIMPLIFICATION:
	 * use this to calculate movement after a unit of time
	 */
		
		double speedA = wheelA.getSpeed();
		double speedB = wheelB.getSpeed();
		
		if (speedA == speedB) {
			wheelB.setSpeed(speedB + 0.0000001);
			// this tries to avoid division by 0
			// but its statistically likely to fail
			// when questioned, say its a feature
		}
		
		double fraction = 
				(distanceBetweenWheels * (speedA + speedB)) 
				/ (2 * (speedA - speedB));
		double trig_x = 
				Math.sin((speedA - speedB) * time
						/ distanceBetweenWheels + initial_angle) - Math.sin(initial_angle);
		double trig_y = 
				Math.cos((speedA - speedB) * time
						/ distanceBetweenWheels + initial_angle) - Math.cos(initial_angle);
		
		
		//this thing calculates location if speed is constant
		//double x = initial_x + fraction * trig_x;
		//double y = initial_y + fraction * trig_y;
		
		double x = getX() + fraction * trig_x;
		double y = getY() + fraction * trig_y;
		
		setX(x);
		setY(y);
		
		double arr[] = {x,y};
		return arr;
	}
	
	
}
